#include "fuzzy_controller.h"
#include <stdio.h>
#include <iomanip>

using namespace std;
using namespace adevs;
using namespace fl;

const double min_time = 15;
const double max_time = 60;

FuzzyController::FuzzyController():
	Controller(),
	remaining_time(10),
	total_time(0),
	current_side_green(-1),
	next_side_green(-1)
{
	sides_count = new int[get_sides_count()];
	phases_skipped = new int[get_sides_count()];
	for(int i =0;i<get_sides_count();i++)
	{
		sides_count[i] = 0;
		phases_skipped[i] = 0;
	}

	init_green_fuzzy();
	init_side_change_fuzzy();
}

void FuzzyController::delta_ext(double e, const Bag<IO_Type>& xb)
{
	remaining_time -= e;
	total_time += e;

	Bag<IO_Type>::const_iterator i = xb.begin();
	for (; i != xb.end(); i++)
	{
		int port = (*i).port;
		int side = get_side_from_input(port);

		if (is_far_sensor(port))
		{
			sides_count[side]++;

			if (side == current_side_green) 
				arivals_during_green++;
		}
		else if (is_near_sensor(port))
		{
			sides_count[side]--;
		}
	}
}
	
void FuzzyController::delta_int()
{
	total_time += ta();

	if (next_side_green == -1)
	{
		int side;
		int lengthonred = get_longest_on_red(side);

		bool increased = false;
		if (current_side_green != -1 && total_time < max_time)
		{
			double remaining_green = 1-(total_time - min_time)/(max_time-min_time);
			double incr = get_green_increase(sides_count[current_side_green], arivals_during_green / total_time, lengthonred, remaining_green);
			if (incr > 0)
			{
				remaining_time = 1;
				increased = true;
			}
		}
		if(!increased)
		{
			next_side_green = get_next_side();
			remaining_time = min_time;
		}
	}
	else // prepnuti semaforu
	{
		//cout << "total time:" << total_time << endl;

		for(int i =0;i<get_sides_count();i++)
		{
			phases_skipped[i] ++;
		}

		total_time = 0;
		arivals_during_green = 0;
		current_side_green = next_side_green;
		next_side_green = -1;

		phases_skipped[current_side_green] = 0;
	}
}

void FuzzyController::delta_conf(const Bag<IO_Type>& xb)
{
}

void FuzzyController::output_func(Bag<IO_Type>& yb)
{
	if (next_side_green != -1) // chceme prepnout
	{
		if (current_side_green != -1)
		{
			IO_Type turn_off(out_semaphor_cmd(current_side_green), (io_type*)new SemaphorCommand(RED));
			yb.insert(turn_off);
		}

		IO_Type turn_on(out_semaphor_cmd(next_side_green), (io_type*)new SemaphorCommand(GREEN));
		yb.insert(turn_on);
	}
}

double FuzzyController::ta()
{
	if (current_side_green == -1 || next_side_green != -1) return 0;

	return remaining_time;
}

void FuzzyController::gc_output(Bag<IO_Type>& g)
{
	Bag<IO_Type>::iterator i;
	for (i = g.begin(); i != g.end(); i++)
	{
		delete (*i).value;
	}
}

FuzzyController::~FuzzyController()
{
	delete green_engine;
}

int FuzzyController::get_longest_on_red(int& side)
{
	int max=-1;
	int s;
	for(int i=0;i<get_sides_count();i++)
	{
		if (i == current_side_green) continue;

		if (sides_count[i] > max)
		{
			max = sides_count[i];
			s = i;
		}
	}

	side = s;
	return max;
}

void FuzzyController::init_green_fuzzy()
{
	FuzzyOperator& op = FuzzyOperator::DefaultFuzzyOperator();
	green_engine = new FuzzyEngine("green_change", op);
		
	FuzzyEngine& engine = *green_engine;
	engine.hedgeSet().add(new fl::HedgeNot);
    engine.hedgeSet().add(new fl::HedgeSomewhat);
    engine.hedgeSet().add(new fl::HedgeVery);

	int max_length = 20;
	lengthOnGreen = new fl::InputLVar("LengthOnGreen");
	lengthOnGreen->addTerm(new fl::ShoulderTerm("VERY_FEW", max_length*0.2, max_length*0.4, true));
	lengthOnGreen->addTerm(new fl::TriangularTerm("FEW", max_length*0.2, max_length*0.6));
	lengthOnGreen->addTerm(new fl::TriangularTerm("MODERATE", max_length*0.4, max_length*0.8));
    lengthOnGreen->addTerm(new fl::ShoulderTerm("MANY", max_length*0.6, max_length*0.8, false));
    engine.addInputLVar(lengthOnGreen);

	//double rof = 1;
	//rateOfArival = new fl::InputLVar("RateOfArival");
	//rateOfArival->addTerm(new fl::ShoulderTerm("SMALL", 0.1*rof, 0.5*rof, true));
	//rateOfArival->addTerm(new fl::TriangularTerm("MODERATE", 0.3*rof, 0.7*rof));
 //   rateOfArival->addTerm(new fl::ShoulderTerm("BIG", 0.5*rof, 0.9*rof, false));
 //   engine.addInputLVar(rateOfArival);

	//max_length = 20;
	longestOnRed = new fl::InputLVar("WaitingOnRed");
	longestOnRed->addTerm(new fl::ShoulderTerm("VERY_FEW", max_length*0.2, max_length*0.4, true));
	longestOnRed->addTerm(new fl::TriangularTerm("FEW", max_length*0.2, max_length*0.6));
	longestOnRed->addTerm(new fl::TriangularTerm("MODERATE", max_length*0.4, max_length*0.8));
    longestOnRed->addTerm(new fl::ShoulderTerm("MANY", max_length*0.6, max_length*0.8, false));
    engine.addInputLVar(longestOnRed);  

	remainingGreen = new fl::InputLVar("RemainingGreen");
	remainingGreen->addTerm(new fl::ShoulderTerm("LOW", 0.1, 0.5, true));
	remainingGreen->addTerm(new fl::TriangularTerm("MEDIUM", 0.1, 0.9));
    remainingGreen->addTerm(new fl::ShoulderTerm("HIGH", 0.5, 0.9, false));
    engine.addInputLVar(remainingGreen);

	green_change = new fl::OutputLVar("GreenChange");
	green_change->addTerm(new fl::TriangularTerm("DECREASE_MORE", -11, -5));
	green_change->addTerm(new fl::TriangularTerm("DECREASE", -8, 0));
	green_change->addTerm(new fl::TriangularTerm("DO_NOT_CHANGE", -5, 5));
	green_change->addTerm(new fl::TriangularTerm("INCREASE", 0, 8));
	green_change->addTerm(new fl::TriangularTerm("INCREASE_MORE", 5, 11));
	engine.addOutputLVar(green_change);

	fl::RuleBlock* block = new fl::RuleBlock();
	block->addRule(new fl::MamdaniRule("if LengthOnGreen is VERY_FEW and WaitingOnRed is VERY_FEW then GreenChange is DO_NOT_CHANGE", engine));
	block->addRule(new fl::MamdaniRule("if LengthOnGreen is FEW and WaitingOnRed is FEW then GreenChange is DO_NOT_CHANGE", engine));
	block->addRule(new fl::MamdaniRule("if LengthOnGreen is MODERATE and WaitingOnRed is MODERATE then GreenChange is DO_NOT_CHANGE", engine));
	block->addRule(new fl::MamdaniRule("if LengthOnGreen is MANY and WaitingOnRed is MANY then GreenChange is DO_NOT_CHANGE", engine));

	block->addRule(new fl::MamdaniRule("if LengthOnGreen is VERY_FEW and WaitingOnRed is FEW then GreenChange is DECREASE", engine));
	block->addRule(new fl::MamdaniRule("if LengthOnGreen is FEW and WaitingOnRed is MODERATE then GreenChange is DECREASE", engine));
	block->addRule(new fl::MamdaniRule("if LengthOnGreen is MODERATE and WaitingOnRed is MANY then GreenChange is DECREASE", engine));

	block->addRule(new fl::MamdaniRule("if LengthOnGreen is VERY_FEW and WaitingOnRed is MODERATE then GreenChange is DECREASE_MORE", engine));
	block->addRule(new fl::MamdaniRule("if LengthOnGreen is FEW and WaitingOnRed is MANY then GreenChange is DECREASE_MORE", engine));

	block->addRule(new fl::MamdaniRule("if LengthOnGreen is VERY_FEW and WaitingOnRed is MANY then GreenChange is DECREASE_MORE", engine));

	block->addRule(new fl::MamdaniRule("if LengthOnGreen is FEW and WaitingOnRed is VERY_FEW then GreenChange is INCREASE", engine));
	block->addRule(new fl::MamdaniRule("if LengthOnGreen is MODERATE and WaitingOnRed is FEW then GreenChange is INCREASE", engine));
	block->addRule(new fl::MamdaniRule("if LengthOnGreen is MANY and WaitingOnRed is MODERATE then GreenChange is INCREASE", engine));

	block->addRule(new fl::MamdaniRule("if LengthOnGreen is MODERATE and WaitingOnRed is VERY_FEW then GreenChange is INCREASE_MORE", engine));
	block->addRule(new fl::MamdaniRule("if LengthOnGreen is MANY and WaitingOnRed is FEW then GreenChange is INCREASE_MORE", engine));

	block->addRule(new fl::MamdaniRule("if LengthOnGreen is MANY and WaitingOnRed is VERY_FEW then GreenChange is INCREASE_MORE", engine));

	//block->addRule(new fl::MamdaniRule("if RateOfArival is SMALL then GreenChange is DECREASE", engine));
	//block->addRule(new fl::MamdaniRule("if RateOfArival is MODERATE then GreenChange is DO_NOT_CHANGE", engine));
	//block->addRule(new fl::MamdaniRule("if RateOfArival is BIG then GreenChange is INCREASE", engine));

	block->addRule(new fl::MamdaniRule("if RemainingGreen is HIGH then GreenChange is INCREASE", engine));
	block->addRule(new fl::MamdaniRule("if RemainingGreen is MEDIUM then GreenChange is DO_NOT_CHANGE", engine));
	block->addRule(new fl::MamdaniRule("if RemainingGreen is LOW then GreenChange is DECREASE", engine));

	engine.addRuleBlock(block);

}

double FuzzyController::get_green_increase(int length_on_green, double rate_of_arival, int longest_on_red, double remaining_green)
{
	lengthOnGreen->setInput(length_on_green);
	//rateOfArival->setInput(rate_of_arival);
	longestOnRed->setInput(longest_on_red);
	remainingGreen->setInput(remaining_green);

	green_engine->process();
	fl::flScalar out = green_change->output().defuzzify();

	//cout << std::setw(3) << length_on_green << " | " << std::setw(3) << longest_on_red << " | " << std::setw(10) << rate_of_arival << " | " << std::setw(10) << remaining_green << " | " << out << endl;

	return out;
}

void FuzzyController::init_side_change_fuzzy()
{
	FuzzyOperator& op = FuzzyOperator::DefaultFuzzyOperator();
	side_engine = new FuzzyEngine("side_change", op);
		
	FuzzyEngine& engine = *side_engine;
	engine.hedgeSet().add(new fl::HedgeNot);
    engine.hedgeSet().add(new fl::HedgeSomewhat);
    engine.hedgeSet().add(new fl::HedgeVery);

	int max_length = 20;
	side_longestOnRed = new fl::InputLVar("LongestOnRed");
	side_longestOnRed->addTerm(new fl::ShoulderTerm("VERY_FEW", max_length*0.2, max_length*0.4, true));
	side_longestOnRed->addTerm(new fl::TriangularTerm("FEW", max_length*0.2, max_length*0.6));
	side_longestOnRed->addTerm(new fl::TriangularTerm("MODERATE", max_length*0.4, max_length*0.8));
    side_longestOnRed->addTerm(new fl::ShoulderTerm("MANY", max_length*0.6, max_length*0.8, false));
    engine.addInputLVar(side_longestOnRed);

	side_queueLength = new fl::InputLVar("QueueLength");
	side_queueLength->addTerm(new fl::ShoulderTerm("VERY_FEW", max_length*0.2, max_length*0.4, true));
	side_queueLength->addTerm(new fl::TriangularTerm("FEW", max_length*0.2, max_length*0.6));
	side_queueLength->addTerm(new fl::TriangularTerm("MODERATE", max_length*0.4, max_length*0.8));
    side_queueLength->addTerm(new fl::ShoulderTerm("MANY", max_length*0.6, max_length*0.8, false));
    engine.addInputLVar(side_queueLength);

	side_skippedPhase = new fl::InputLVar("SkippedPhase");
	side_skippedPhase->addTerm(new fl::ShoulderTerm("LOW", 1, 4, true));
	side_skippedPhase->addTerm(new fl::TriangularTerm("MEDIUM", 1, 7));
    side_skippedPhase->addTerm(new fl::ShoulderTerm("HIGH", 4, 7, false));
    engine.addInputLVar(side_skippedPhase);

	side_priority = new fl::OutputLVar("SidePriority");
	side_priority->addTerm(new fl::TriangularTerm("LOW", -8, 0));
	side_priority->addTerm(new fl::TriangularTerm("MEDIUM", -5, 5));
	side_priority->addTerm(new fl::TriangularTerm("HIGH", 0, 8));
	engine.addOutputLVar(side_priority);

	fl::RuleBlock* block = new fl::RuleBlock();
	block->addRule(new fl::MamdaniRule("if QueueLength is VERY_FEW and LongestOnRed is VERY_FEW then SidePriority is HIGH", engine));
	block->addRule(new fl::MamdaniRule("if QueueLength is FEW and LongestOnRed is FEW then SidePriority is HIGH", engine));
	block->addRule(new fl::MamdaniRule("if QueueLength is MODERATE and LongestOnRed is MODERATE then SidePriority is HIGH", engine));
	block->addRule(new fl::MamdaniRule("if QueueLength is MANY and LongestOnRed is MANY then SidePriority is HIGH", engine));

	block->addRule(new fl::MamdaniRule("if QueueLength is VERY_FEW and LongestOnRed is FEW then SidePriority is MEDIUM", engine));
	block->addRule(new fl::MamdaniRule("if QueueLength is FEW and LongestOnRed is MODERATE then SidePriority is MEDIUM", engine));
	block->addRule(new fl::MamdaniRule("if QueueLength is MODERATE and LongestOnRed is MANY then SidePriority is MEDIUM", engine));

	block->addRule(new fl::MamdaniRule("if QueueLength is VERY_FEW and LongestOnRed is MODERATE then SidePriority is LOW", engine));
	block->addRule(new fl::MamdaniRule("if QueueLength is FEW and LongestOnRed is MANY then SidePriority is LOW", engine));

	block->addRule(new fl::MamdaniRule("if SkippedPhase is LOW then SidePriority is LOW", engine));
	block->addRule(new fl::MamdaniRule("if SkippedPhase is MEDIUM then SidePriority is MEDIUM", engine));
	block->addRule(new fl::MamdaniRule("if SkippedPhase is HIGH then SidePriority is HIGH", engine));

	engine.addRuleBlock(block);
}

int FuzzyController::get_next_side()
{
	if (current_side_green == -1) return 0;

	int s;
	int longest = get_longest_on_red(s);

	int next_side = 0;
	double max_priority = -1000;

	for(int side = 0; side < get_sides_count(); side++)
	{
		if (side == current_side_green)
			continue;

		double priority = get_side_priority(longest, sides_count[side], phases_skipped[side]);

		if (priority > max_priority)
		{
			next_side = side;
			max_priority = priority;
		}
	}

	return next_side;
}

double FuzzyController::get_side_priority(int longest_on_red, int queue_length, int skipped_phases)
{
	side_longestOnRed->setInput(longest_on_red);
	side_queueLength->setInput(queue_length);
	side_skippedPhase->setInput(skipped_phases);

	side_engine->process();
	fl::flScalar out = side_priority->output().defuzzify();

	//cout << std::setw(3) << longest_on_red << " | " << std::setw(10) << queue_length << " | " << std::setw(3) << skipped_phases <<  " | " << out << endl;

	return out;
}